#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import roslib;
import rospy  
import actionlib  
import random
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
from std_msgs.msg import Int8

STATUS_EXPLORING    = 0
STATUS_CLOSE_TARGET = 1
STATUS_GO_HOME      = 2

# 设置检索顺序点
search_pos=[[0,8],[4,8],[4,0],[8,0],[8,8]]

class ExploringMaze():
    def __init__(self):  
        rospy.init_node('exploring_maze', anonymous=True)  
        rospy.on_shutdown(self.shutdown)  

        # 在每个目标位置暂停的时间 (单位：s)
        self.rest_time = rospy.get_param("~rest_time", 0.5)  

        # 发布控制机器人的消息  
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 
 
        # 创建一个Subscriber，订阅/exploring_cmd
        rospy.Subscriber("/exploring_cmd", Int8, self.cmdCallback)

        # 订阅move_base服务器的消息  
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  

        rospy.loginfo("Waiting for move_base action server...")  
        
        # 60s等待时间限制  
        self.move_base.wait_for_server(rospy.Duration(60))  
        rospy.loginfo("Connected to move base server")  
 
        # 保存运行时间的变量   
        start_time = rospy.Time.now()  
        running_time = 0  

        rospy.loginfo("开始探索迷宫")  
        
        # 初始位置
        start_location = Pose(Point(0, 0, 0), Quaternion(0.000, 0.000, 0.709016873598, 0.705191515089))  
 
        # 命令初值
        self.exploring_cmd = STATUS_EXPLORING
       
        # 用于检索顺序
        self.index=0;
        # 开始主循环，随机导航  
        while not rospy.is_shutdown():
######################################请补充主循环中的代码（开始）######################################################

            #设定下一个随机目标点
            self.goal = MoveBaseGoal()
            self.goal.target_pose.pose = start_location
            self.goal.target_pose.header.frame_id = 'map'
            self.goal.target_pose.header.stamp = rospy.Time.now()

            if self.exploring_cmd is STATUS_EXPLORING:
                rospy.loginfo("设置目标点")  
                self.goal.target_pose.pose.position.x = search_pos[self.index][0]
                self.goal.target_pose.pose.position.y = search_pos[self.index][1]
                self.index+=1
            elif self.exploring_cmd is STATUS_CLOSE_TARGET:
                rospy.sleep(1)
                continue

            #让用户知道下一个位置
            rospy.loginfo("Going to:" + str(self.goal.target_pose.pose))

            #向下一个位置进发
            self.move_base.send_goal(self.goal)

            #五分钟时间限制
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))

            #查看是否成功到达
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("超时未能到达目标点")
            else:
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    if self.exploring_cmd is STATUS_GO_HOME:
                        rospy.loginfo("返航完成,迷宫寻宝结束!")
                        self.shutdown()
                    else:
                        rospy.loginfo("到达目标点!")
                else:
                    rospy.loginfo("未能到达目标点!")

            #运行所用时间
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0

            #输出本次导航的所有信息
            # rospy.loginfo("Current time:" + str(trunc(running_time,1)) + "min")
            

######################################请补充主循环中的代码（结束）######################################################
        self.shutdown()

    def cmdCallback(self, msg):
        rospy.loginfo("get state: %d", msg.data)
        self.exploring_cmd = msg.data

        if self.exploring_cmd is STATUS_CLOSE_TARGET:
            rospy.loginfo("停止机器...")  
            self.move_base.cancel_goal() 
        elif self.exploring_cmd is STATUS_GO_HOME:
            self.goal.target_pose.pose.position.x = 0
            self.goal.target_pose.pose.position.y = 0
            #向回程位置进发
            self.move_base.send_goal(self.goal)
            rospy.loginfo("回程参数设置成功，开始返航") 

    def shutdown(self):  
        rospy.loginfo("停止机器...")  
        self.move_base.cancel_goal()  
        rospy.sleep(2)  
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)  

def trunc(f, n):  
    slen = len('%.*f' % (n, f))  
    return float(str(f)[:slen])  

if __name__ == '__main__':  
    try:  
        ExploringMaze()  
        rospy.spin()  

    except rospy.ROSInterruptException:  
        rospy.loginfo("Exploring maze finished.")
